#include "Scythe.h"
#include "Stream.h"

namespace Scythe
{
const int MAX_SHAPE_PER_ACTOR = 40;


inline Vector3 toScy(const NxVec3 &v) 
{
	return Vector3(v.x, v.y, v.z);
}
inline Matrix toScy(const NxMat33 &m) 
{
	return *(Matrix*)&m;
}
inline Matrix toScyRows(const NxMat33 &m) 
{
	Matrix s;
	s.m[0]		= toScy(m.getRow(0));
	s.m[1]		= toScy(m.getRow(1));
	s.m[2]		= toScy(m.getRow(2));
	return s;
}



Vector3 PhysicalObject::getModelPos(int id)
{
	if(models[id]->rigidbody < 0)
		return Vector3(0,0,0);

	Vector3 rigidPos = toScy(models[id]->rigidbody->body->getGlobalPosition());

	//get orientation
	Matrix m33 = toScy(models[id]->rigidbody->body->getGlobalOrientation());
/*
	m33 = m33 * models[id]->offsetMat;
	
	//convert m33 from NxMat to a Ogre quat
	node[i]->setOrientation( nxMatToNode(m33) );
*/

	return getModelPos(id, rigidPos, m33);
}




Matrix	PhysicalObject::getModelRotRows(int id)
{
	if(models[id]->rigidbody < 0)
		return Matrix();

	//I think this is right, but you may want to check it if you are using it and having orientation problems

	//get orientation
	Matrix  m33 = toScyRows(models[id]->rigidbody->body->getGlobalOrientation());
	m33 = m33 * models[id]->offsetMat;

	return m33;
}


Matrix	PhysicalObject::getModelRotColumns(int id)
{
	if(models[id]->rigidbody < 0)
		return Matrix();

	//get orientation
	Matrix  m33 = toScy(models[id]->rigidbody->body->getGlobalOrientation());
	m33 = m33 * models[id]->offsetMat;

	return m33;
}


Quat	PhysicalObject::getModelRotQuat(int id)
{
	if(models[id]->rigidbody < 0)
		return Quat();

	//get orientation
	return getModelRotQuat(id, toScy(models[id]->rigidbody->body->getGlobalOrientation()));
}


}